/**
 *
 */
#include <Metro.h> //Include Metro library
#include <Servo.h>
#include <Messenger.h>

enum{
	POS_NTL = 0,
	POS_PSH,
	POS_SET,
	POS_NUM
};

#define SERVO_NUM	3
static const int servo_pin[SERVO_NUM] = {11, 10, 9};
static const int servo_degs[SERVO_NUM][POS_NUM] = {{41, 34, 120}, {62, 53, 120}, {49, 42, 120}};
#define COMMAND_DRUM_NUM  4 /* 1コマンドの太鼓数 */


/* INTERVAL */
#define INTERVAL_SERVO_PUSH	200
#define INTERVAL_1PUSH		500	
#define INTERVAL_WAITING	INTERVAL_1PUSH * 4

/*
 * |--       |--       |--       |--         <- サーボを動かしている間の時間(SERVO_PUSH)
 * |---------|---------|---------|---------  <- 1サイクルのPUSH4セット+その間(1PUSH)
 *  サイクル間の待ち時間(WAITING) ->       ----------------------------------------
 */

enum{
	ST_INI,
	ST_ACT_PSH,
	ST_ACT_REL,
	ST_WAIT,
	ST_CALLIBRATION
};

/* コマンド */
enum{
	BT_PT = 0,
	BT_PO,
	BT_CH,
	BT_NUM
};
enum{
	COMMAND_ADVANCE = 0,
	COMMAND_POOLSTR,
	COMMAND_ATTACK,
	COMMAND_GUARD,
	COMMAND_TYPE_NUM
};
static const int command_pattern[COMMAND_TYPE_NUM][COMMAND_DRUM_NUM] =
{
	{BT_PT, BT_PT, BT_PT, BT_PO},	// ADVANCE
	{BT_PO, BT_PO, BT_CH, BT_CH},	// POOLSTR
	{BT_PO, BT_PO, BT_PT, BT_PO},	// ATTACK
	{BT_CH, BT_CH, BT_PT, BT_PO},	// GUARD
};

/* コマンドシーケンスパターン */
enum{
	SEQ_POOLATK,
	SEQ_GUARD,
	SEQ_ADVANCE,
	SEQ_PTN_NUM
};
#define SEQUENCE_NUM	2
/* XXX 1シーケンス2コマンドで統一化してしまう
 * 1シーケンス1コマンドでいいものは2個繰り返し
 */
static const int scene[SEQ_PTN_NUM][SEQUENCE_NUM] =
{
	{COMMAND_POOLSTR, COMMAND_ATTACK},	// POOLATK
	{COMMAND_GUARD, COMMAND_GUARD},		// GUARD
	{COMMAND_ADVANCE, COMMAND_ADVANCE},	// ADVANCE
};

/* descriptor */
typedef struct{
	int state;
	int sequence;		/* どのコマンドシーケンスを使用するか(外から指定) */
	unsigned int count; /* 総合Pushカウント(単純カウントアップ) */
} T_AUTO_PUSH_DESC;
static T_AUTO_PUSH_DESC desc;

Servo myservo[SERVO_NUM];
Metro m_pushing = Metro(INTERVAL_SERVO_PUSH);
Metro m_cycle_interval = Metro(INTERVAL_WAITING);
Metro m_push_interval = Metro(INTERVAL_1PUSH);
Messenger message = Messenger();

// static functions
static void change_state(int next_state);
static void servo_action(int sequence, unsigned int count, int push_ntl);
static void servo_ini(int pos);

/**
 * シリアルからのメッセージ受信完了
 */
void messageCompleted()
{
	int servo_num;
	int deg;
	while(message.available()){
		if(desc.state == ST_CALLIBRATION){
			if(message.checkString("s")){
				servo_num = message.readInt();
				deg = message.readInt();
				myservo[servo_num].write(deg);
			}
			else if(message.checkString("e")){
				Serial.println("exit callibration mode");
				change_state(ST_INI);
			}
		}
		else if(desc.state != ST_INI){
			/* INIステート以外で何かキャラクタ受信したら初期状態に戻る */
			change_state(ST_INI);
		}
		else{
			if(message.checkString("s")){
				Serial.println("set position");
				servo_ini(POS_SET);
			}
			else if(message.checkString("n")){
				Serial.println("neutral position");
				servo_ini(POS_NTL);
			}
			else if(message.checkString("c")){
				Serial.println("callibration mode");
				Serial.println("  s [servo_num] [deg]");
				Serial.println("  e : exit callibration mode");
				change_state(ST_CALLIBRATION);
			}
			else if(message.checkString("0")){
				Serial.println("start 0 sequence");
				desc.sequence = 0;
				desc.count = 0;
				change_state(ST_ACT_PSH);
			}
			else if(message.checkString("1")){
				Serial.println("start 1 sequence");
				desc.sequence = 1;
				desc.count = 0;
				change_state(ST_ACT_PSH);
			}
			else if(message.checkString("2")){
				Serial.println("start 2 sequence");
				desc.sequence = 2;
				desc.count = 0;
				change_state(ST_ACT_PSH);
			}
		}
		/* 読み捨て */
		message.readChar();
	}
}

/**
 * 初期化処理
 * arduinoではこの関数は起動時の1度だけ呼ばれる
 */
void setup()
{
	servo_ini(POS_NTL);
	Serial.begin(115200);
	message.attach(messageCompleted);

	desc.state = ST_INI;
}

/**
 * メインループ
 * arduinoではこの関数が繰り返し呼ばれる
 * ステートマシンのアイドル処理
 * 主にタイムアウトチェックとシリアルからの指示受付待ち
 */
void loop()
{
	char rcv;

	/* シェルアイドル処理(改行コード受信でmessageCompleted関数をコール) */
	while(Serial.available()) message.process(Serial.read());

	switch(desc.state){
	case ST_INI:
		break;

	case ST_ACT_PSH:
		if(m_pushing.check()){
			change_state(ST_ACT_REL);
		}
		break;

	case ST_ACT_REL:
		if(m_push_interval.check()){
			/* 4の剰余が0(Push4回毎にWAITステートへ) */
			if(!(desc.count % COMMAND_DRUM_NUM)){
				change_state(ST_WAIT);
			}
			else{
				change_state(ST_ACT_PSH);
			}
		}
		break;

	case ST_WAIT:
		if(m_cycle_interval.check()){
			change_state(ST_ACT_PSH);
		}
		break;
	}
}

/**
 * ステート変更
 * 最初に遷移すべきステートにジャンプするので、
 * switch内の各処理はSTM図で言うところのentryに当たる
 */
static void change_state(int next_state)
{
	//Serial.print("st: ");	Serial.print(desc.state);	Serial.print(" -> ");	Serial.println(next_state);

	desc.state = next_state;
	// ステート遷移時の処理を書く
	switch(next_state){
	case ST_INI:
		servo_ini(POS_NTL);
		break;
	case ST_ACT_PSH:
		//Serial.print("num: "); Serial.println(desc.count);
		servo_action(desc.sequence, desc.count, POS_PSH);
		m_pushing.reset();
		m_push_interval.reset();
		break;
	case ST_ACT_REL:
		servo_action(desc.sequence, desc.count, POS_NTL);
		desc.count++;
		break;
	case ST_WAIT:
		if((desc.count % 12) == 0){
			/* 2001ms待ちじゃ遅すぎ、2000msじゃ早すぎるようなので微調整 */
			m_cycle_interval.interval(INTERVAL_1PUSH * 4 + 2);
		}
		else{
			m_cycle_interval.interval(INTERVAL_1PUSH * 4);
		}
		m_cycle_interval.reset();
		break;
	}
}

/*
 * 言葉の定義
 *
 * sequence: PCから指定する一連の流れ(タメ攻撃とか、ずっと守りとか)を表す。複数のコマンドからなる
 * command: 4拍のサーボ制御からなる1つの動作命令(攻撃、守り、タメ等)を表す。どのサーボを制御するかの組み合わせからなる
 * button: どのサーボがどのボタンに割当たっているかを表す定義。PT, PO, CH, DOの4種がある(実際に使用しているのは3種のみ)
 */
